/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoystick;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.Talon;

/**
 *
 * @author Robotics
 */

public class Chassis extends Subsystem {
    RobotDrive drive;
    Encoder FrontLeftEncoder, FrontRightEncoder, RearLeftEncoder, RearRightEncoder;
    Talon FrontLeftMotor, BackLeftMotor, FrontRightMotor, BackRightMotor;
    double LeftAverage = 0, RightAverage = 0;
    int currAmountLeft = 0, currAmountRight = 0;
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        setDefaultCommand(new DriveWithJoystick());// Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public Chassis(){
        
        FrontLeftEncoder = new Encoder(RobotMap.FRONT_LEFT_ENCODER_PORTA_DIO, RobotMap.FRONT_LEFTT_ENCODER_PORTB_DIO, false, EncodingType.k2X);
        FrontRightEncoder = new Encoder(RobotMap.FRONT_RIGHT_ENCODER_PORTA_DIO, RobotMap.FRONT_RIGHT_ENCODER_PORTB_DIO, false, EncodingType.k2X);
        RearLeftEncoder = new Encoder(RobotMap.REAR_LEFT_ENCODER_PORTA_DIO, RobotMap.REAR_LEFT_ENCODER_PORTB_DIO, false, EncodingType.k2X);
        RearRightEncoder = new Encoder(RobotMap.REAR_RIGHT_ENCODER_PORTA_DIO, RobotMap.REAR_RIGHT_ENCODER_PORTB_DIO, false, EncodingType.k2X);
        
        //drive = new RobotDrive(RobotMap.RIGHTMOTOR, RobotMap.LEFTMOTOR);
        //drive = new RobotDrive(1, 3, 2, 4);
        drive = new RobotDrive(RobotMap.FRONT_LEFTMOTOR_PWM, RobotMap.REAR_LEFTMOTOR_PWM, RobotMap.FRONT_RIGHTMOTOR_PWM, RobotMap.REAR_RIGHTMOTOR_PWM);
        drive.setSafetyEnabled(false);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        
        FrontLeftEncoder.start();
        FrontRightEncoder.start();
        RearLeftEncoder.start();
        RearRightEncoder.start();
    }
    
    
    public void straight(double Angle) {
        drive.mecanumDrive_Cartesian(0, 1.0 / 1.0, 0, Angle);      
        updateEncoders();
    }
    
    public void turnLeft() {
        drive.mecanumDrive_Cartesian(1.0 / 1.0, 0, 0, 0); //NEED TO CALCULATE VALUES
        updateEncoders();
    }
    
    public void turnRight(){
        drive.mecanumDrive_Cartesian(1.0 / 1.0, 0, 0, 0); //NEED TO CALCULATE VALUES
        updateEncoders();
    }
    
    public void turnAngled(double Angle){
        drive.mecanumDrive_Polar(1.0 / 1.0, Angle, 0);
        updateEncoders();
    }
    
    public void strafeLeft(){
        drive.mecanumDrive_Polar(1.0 / 6.0, -90, 0);
        updateEncoders();
    }
    
    public void strafeRight(){
        drive.mecanumDrive_Polar(1.0 / 6.0, 90, 0);
        updateEncoders();
    }
    
    public void resetEncoders(){
        FrontLeftEncoder.reset();
        FrontRightEncoder.reset();
        RearLeftEncoder.reset();
        RearRightEncoder.reset();
    }
    
    public void driveWithJoystick(double Magnitude, double Degrees, double Twist){
        //drive.mecanumDrive_Polar(stick.getMagnitude() / 8, stick.getDirectionDegrees(), stick.getTwist()); 
        //drive.mecanumDrive_Cartesian(stick.getX() / 8, stick.getY() / 8, stick.getTwist(), 0);
        //drive.arcadeDrive(stick);
        
        drive.mecanumDrive_Polar(Magnitude / 1.0, Degrees, Twist);
        
        SmartDashboard.putNumber("Magnitude: ", Magnitude / 1.0);
        SmartDashboard.putNumber("Degrees: ", Degrees);
        SmartDashboard.putNumber("Twist: ", Twist);
        updateEncoders();
     }
    
    public double getFrontLeftEncoderValue(){
        return FrontLeftEncoder.get();
    }
    
    public double getFrontRightEncoderValue(){
        return FrontRightEncoder.get();
    }
    
    public double getRearLeftEncoderValue(){
        return RearLeftEncoder.get();
    }
    
    public double getRearRightEncoderValue(){
        return RearRightEncoder.get();
    }
    
    public void updateEncoders(){
        SmartDashboard.putNumber("Front Left Encoder Count: ", getFrontLeftEncoderValue());
        SmartDashboard.putNumber("Front Right Encoder Count: ", getFrontRightEncoderValue());
        SmartDashboard.putNumber("Rear Left Encoder Count: ", getRearLeftEncoderValue());
        SmartDashboard.putNumber("Rear Right Encoder Count: ", getRearRightEncoderValue());
        SmartDashboard.putNumber("Front Left Encoder Rate: ", FrontLeftEncoder.getRate());
        SmartDashboard.putNumber("Front Right Encoder Rate: ", FrontRightEncoder.getRate());
        SmartDashboard.putNumber("Rear Left Encoder Rate: ", RearLeftEncoder.getRate());
        SmartDashboard.putNumber("Rear Right Encoder Rate: ", RearRightEncoder.getRate());
        //used for tuning mecanum wheels
        /*LeftAverage += FrontLeftEncoder.getRate();
        RightAverage += FrontRightEncoder.getRate();
        currAmountLeft++;
        currAmountRight++;
        LeftAverage /= currAmountLeft;
        RightAverage /= currAmountRight;*/
        //double LeftAverage += FrontLeftEncoder.getRate(), RightAverage += FrontRightEncoder.getRate();
        /*double currLeft = FrontLeftEncoder.getRate(), currRight = FrontRightEncoder.getRate();
        System.out.println("Curr Left Rate: " + currLeft);
        System.out.println("Curr Right Rate: " + currRight);
        System.out.println("Curr Difference: " + (Math.abs(currLeft) - Math.abs(currRight)));
        System.out.println("");*/
        /*System.out.println("Curr Left Avg: " + LeftAverage);
        System.out.println("Curr Right Avg: " + RightAverage);
        System.out.println("Difference Avg: " + (LeftAverage - RightAverage));*/
    }
}